/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package deatax.math;

/**
 *
 * @author pson
 */
public class Quaternion {
    
    public float w;
    public float x;
    public float y;
    public float z;

    public Quaternion() {
        w = 0;
        x = 0;
        y = 0;
        z = 0;
    }

    public Quaternion(float x, float y, float z, float w) {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
    }
    
    public Quaternion(Quaternion q) {
        this.w = q.w;
        this.x = q.x;
        this.y = q.y;
        this.z = q.z;
    }
    
    public Quaternion(Matrix3 m) {
        set(m);
    }
    
    /**
     * From euler angles
     * @param x
     * @param y
     * @param z 
     */
    public Quaternion(float x, float y, float z) {
        final float sx = (float)Math.sin(x * 0.5f);
        final float cx = (float)Math.cos(x * 0.5f);
        final float sy = (float)Math.sin(y * 0.5f);
        final float cy = (float)Math.cos(y * 0.5f);
        final float sz = (float)Math.sin(z * 0.5f);
        final float cz = (float)Math.cos(z * 0.5f);
        
        final float cxcy = cx * cy;
        final float sxsy = sx * sy;
        
        this.x = sz * cxcy - cz * sxsy;
        this.y = cz * sx * cy + sz * cx * sy;
        this.z = cz * cx * sy - sz * sx * cy;
        this.w = cz * cxcy + sz * sxsy;
        
    }
    
    //Do matrix3 rep of quaternion
    
    /**
     * axis angle constructor
     * @param axis
     * @param angle 
     */
    public Quaternion(Vector3 axis, float angle) {
        axis = axis.normalised();
        
        final float sa = (float)Math.sin(angle / 2);
        final float ca = (float)Math.cos(angle / 2);
        
        this.x = axis.x * sa;
        this.y = axis.y * sa;
        this.z = axis.z * sa;
        this.w = ca;
    }
    
    /**
     * Vector3 constructor. Must be normalised
     * @param v 
     */
    public Quaternion(Vector3 v) {
        this.x = v.x;
        this.y = v.y;
        this.z = v.z;
        
        final float t = 1.0f - (x*x) - (y*y) - (z*z);
        if (t < 0.0f) {
            w = 0;
        } else {
            w = (float)Math.sqrt(t);
        }
    }
    
    public final void set(Matrix3 m) {
        float[] mv = m.v;
        float t = 1 + mv[0] + mv[4] + mv[8];
        
        if (t > 0.001f) {
            float s = (float)Math.sqrt(t) * 2.0f;
            x = (mv[7] - mv[5]) / s;
            y = (mv[2] - mv[6]) / s;
            z = (mv[3] - mv[1]) / s;
            w = 0.25f * s;
        } else if (mv[0] > mv[4] && mv[0] > mv[8]) {
            float s = (float)Math.sqrt(1.0f + mv[0] - mv[4] - mv[8]) * 2.0f;
            x = 0.25f * s;
            y = (mv[3] - mv[1]) / s;
            z = (mv[2] - mv[6]) / s;
            w = (mv[7] - mv[5]) / s;
        } else  if (mv[4] > mv[8]) {
            float s = (float)Math.sqrt(1.0f + mv[4] - mv[0] - mv[8]) * 2.0f;
            x = (mv[3] - mv[1]) / s;
            y = 0.25f * s;
            z = (mv[7] - mv[5]) / s;
            w = (mv[2] - mv[6]) / s;
        } else {
            float s = (float)Math.sqrt(1.0f + mv[8] - mv[0] - mv[4] * 2.0f);
            x = (mv[2] - mv[6]) / s;
            y = (mv[7] - mv[5]) / s;
            z = 0.25f * s;
            w = (mv[3] - mv[1]) / s;
        }
    }
    
    public Quaternion interpolate(Quaternion s, Quaternion f, float t) {
        float co = s.x * f.x + s.y * f.y + s.z * f.z + s.w * f.w;
        
        Quaternion ft = new Quaternion(f);
        if (co < 0.0f) {
            co = -co;
            ft.x = -ft.x;
            ft.y = -ft.y;
            ft.z = -ft.z;
            ft.w = -ft.w;
        }
        
        float p;
        float q;
        
        if ((1.0f - co) > 0.0001f) {
            float o = (float)Math.acos(co);
            float so = (float)Math.sin(o);
            p = (float)Math.sin((1.0f - t) * o) / so;
            q = (float)Math.sin(t * o) / so;
        } else {
            p = 1.0f - t;
            q = t;
        }
        
        return new Quaternion(
                p * s.x + q * f.x, 
                p * s.y + q * f.y, 
                p * s.z + q * f.z,
                p * s.w + q * f.w);
    }
    
    public Quaternion normalised() {
        final float n = x*x+y*y+z*z+w*w;
        if (n != 0.0f) {
            return new Quaternion(x/n, y/n, z/n, w/n);
        }
        return new Quaternion(this);
        
    }
    
    public Quaternion conjugated() {
        return new Quaternion(-x, -y, -z, w);
    }
    
    public Quaternion multiply(Quaternion q) {
        return new Quaternion(
                w*q.x + x*q.w + y*q.z - z*q.y, 
                w*q.y + y*q.w + z*q.x - x*q.z,
		w*q.z + z*q.w + x*q.y - y*q.x,
                w*q.w - x*q.x - y*q.y - z*q.z);
    }
    
    public Vector3 rotated(Vector3 v) {
        Quaternion q2 = new Quaternion(v.x, v.y, v.z, 0.0f);
        
        Quaternion q = conjugated();
        
        q = q.multiply(q2).multiply(this);
        return new Vector3(q.x, q.y, q.z);
    }
}
